#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/spatial/explog.hpp"

#include <iostream>

Eigen::MatrixXd pseudoInv_SVD(const Eigen::MatrixXd &mat);
Eigen::Matrix<double,3,3> eul2Rot(double roll, double pitch, double yaw); // 欧拉角转旋转矩阵，ZYX顺序
Eigen::Vector3d Rot2eul(const Eigen::Matrix3d &rot);
double clip(double dataIn, double upperLim, double lowerLim); // 限副函数
Eigen::MatrixXd pseudoInv_SVD(const Eigen::MatrixXd &mat);
double adjustNumber(double num, double lowerBound, double upperBound);
double computeArmAngleJacobian(const Eigen::Vector3d &v_elbow, const Eigen::Vector3d &v_end,
							   const Eigen::MatrixXd &Jelbow, Eigen::MatrixXd &JarmAngle );
double computeArmAngle(const Eigen::Vector3d &v_elbow, const Eigen::Vector3d &v_end);

int main(int argc, char ** argv)
{
	// 导入urdf模型，初始化数据容器
	const std::string urdf_robot_filename = std::string("../model/AzureLoong/AzureLoong.urdf");
	pinocchio::Model model_biped;
	pinocchio::urdf::buildModel(urdf_robot_filename, model_biped);
//	pinocchio::urdf::buildModel(urdf_robot_filename, root_joint, model_biped);
	pinocchio::Data data_biped(model_biped);

	// display all the joints' ids and names
	// 注意，这个库会在头上加个universe的joint，导致这里显示的joint个数比urdf里面的多一个
	// 但是在存放关节位置中的q向量中，加的这个关节是不算个数的，因此用此处显示的jointid去q中读取数据时要减1
	for(pinocchio::JointIndex jointIdx = 1; jointIdx < (pinocchio::JointIndex)model_biped.njoints; jointIdx++)
		std::cout << std::setw(10) << std::left
				  << jointIdx<<' '<< model_biped.names[jointIdx] << ' ' << model_biped.lowerPositionLimit[jointIdx-1] <<
				  ' '<< model_biped.upperPositionLimit[jointIdx-1]<<std::endl;

	// get joint id according to the joint name
	auto l_knee_Joint=model_biped.getJointId("J_knee_l_pitch");
	auto l_ankle_Joint=model_biped.getJointId("J_ankle_l_roll");
	auto r_knee_Joint=model_biped.getJointId("J_knee_r_pitch");
	auto r_ankle_Joint=model_biped.getJointId("J_ankle_r_roll");

	// set desired rotation matrix Rdes and position Pdes, all are relative to the base frame
	Eigen::Matrix3d Rdes_l, Rdes_r;
	Eigen::Vector3d Pdes_l, Pdes_r;
	int index_waist = 16; // the id of q that will be kept the initial value during the iteration
	int dof_waist = 3; // the id of q that will be kept the initial value during the iteration

	Rdes_l = eul2Rot(0., 0., 0.);
	Pdes_l << 0.2,0.3,-0.97;  // for left arm, offset pos is [-0.0013, 0.2645, -0.1210]
	Rdes_r = eul2Rot(0., 0., 0.);
	Pdes_r << 0.2,-0.3,-0.97;  // for left arm, offset pos is [-0.0013, -0.2645, -0.1210]
	const pinocchio::SE3 oMdes_l(Rdes_l, Pdes_l);
	const pinocchio::SE3 oMdes_r(Rdes_r, Pdes_r);

	// set ini pos
	// 尤其注意要把肘关节的初始值设置成期望朝里或者朝外的角度
	Eigen::VectorXd q = Eigen::VectorXd::Zero(model_biped.nv);
	Eigen::VectorXd delta_q = Eigen::VectorXd::Zero(model_biped.nv);
	q(l_knee_Joint-1) = -0.1;
	q(r_knee_Joint-1) = -0.1;


	// set solver parameters
	const double eps = 1e-4;
	const int IT_MAX = 100;
	const double DT = 7e-1;
	const double damp = 5e-3;
	// variables ini
	Eigen::MatrixXd Jend_l(6, model_biped.nv);
	Jend_l.setZero();
	Eigen::MatrixXd Jend_r(6, model_biped.nv);
	Jend_r.setZero();
	Eigen::MatrixXd Jend_l_new(6, model_biped.nv);
	Jend_l_new.setZero();
	Eigen::MatrixXd Jend_r_new(6, model_biped.nv);
	Jend_r_new.setZero();
	Eigen::MatrixXd Jend(12, model_biped.nv);
	Jend.setZero();

	bool success = false;
	Eigen::Matrix<double,12,1> err;
	Eigen::Matrix<double,6,1> err_l, err_r;
	Eigen::VectorXd v(model_biped.nv);

	int itr_count{0};
	// iteration loop
	for (itr_count=0;; itr_count++)
	{
		// calculate forward kinematics and posture errors
		pinocchio::forwardKinematics(model_biped, data_biped, q);
		const pinocchio::SE3 iMd_l = data_biped.oMi[l_ankle_Joint].actInv(oMdes_l);
		const pinocchio::SE3 iMd_r = data_biped.oMi[r_ankle_Joint].actInv(oMdes_r);

		err_l = pinocchio::log6(iMd_l).toVector();  // in joint frame
		err_r = pinocchio::log6(iMd_r).toVector();  // in joint frame
		err.block(0,0,6,1)=err_l;
		err.block(6,0,6,1)=err_r;

		// finish conditions
		if(err.norm() < eps)
		{
			success = true;
			break;
		}
		if (itr_count > IT_MAX-1)
		{
			success = false;
			break;
		}

		// calculate delta_q
		pinocchio::computeJointJacobians(model_biped, data_biped, q);  // Jend in joint frame
		pinocchio::getJointJacobian(model_biped, data_biped, l_ankle_Joint, pinocchio::LOCAL, Jend_l);  // Jend in joint frame
		pinocchio::getJointJacobian(model_biped, data_biped, r_ankle_Joint, pinocchio::LOCAL, Jend_r);  // Jend in joint frame
		Jend_l.block(0,index_waist,6,dof_waist).setZero();
		Jend_r.block(0,index_waist,6,dof_waist).setZero();

		pinocchio::Data::Matrix6 Jlog_l;
		Jlog_l.setZero();
		pinocchio::Jlog6(iMd_l.inverse(), Jlog_l);
		Jend_l_new = -Jlog_l * Jend_l;
		pinocchio::Data::Matrix6 Jlog_r;
		Jlog_r.setZero();
		pinocchio::Jlog6(iMd_r.inverse(), Jlog_r);
		Jend_r_new = -Jlog_r * Jend_r;
		Jend.block(0,0,6,model_biped.nv)=Jend_l_new;
		Jend.block(6,0,6,model_biped.nv)=Jend_r_new;

		Eigen::MatrixXd JJt(Jend.rows(),Jend.rows());
		JJt.noalias() = Jend * Jend.transpose();//伪逆
		JJt.diagonal().array() += damp;//加阻尼

		v.noalias() = - Jend.transpose() * JJt.ldlt().solve(err);
//		v.noalias() = pseudoInv_SVD(Jend)*err;
		delta_q = v*DT;

		q += delta_q;

//		if(!(itr_count % 10)){
//			std::cout << itr_count << ": error = " << err.transpose() << std::endl;
//		}

		std::cout << q.transpose() << ";" <<std::endl;
	}

	if(success)
	{
		for (int i=0;i<model_biped.nv;i++){
			q[i] = adjustNumber(q[i], model_biped.lowerPositionLimit[i],model_biped.upperPositionLimit[i]);
		}
		std::cout << "Convergence achieved!" << std::endl;
		std::cout << "error = " << err.transpose() << std::endl;
		std::cout<< "itr count= "<<itr_count<<std::endl;
	}
	else
	{
		std::cout << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" << std::endl;
	}

	std::cout << "\nresult: " << q.transpose() << std::endl;
	std::cout << "\n fe_Pos: " << data_biped.oMi[l_ankle_Joint].translation().transpose() << ", "
			  <<data_biped.oMi[r_ankle_Joint].translation().transpose();
	std::cout << "\n fe_Eul: " << Rot2eul(data_biped.oMi[l_ankle_Joint].rotation()).transpose() << ", "
			  <<Rot2eul(data_biped.oMi[r_ankle_Joint].rotation()).transpose();
//	std::cout << "\n elbowPos: "<< data_biped.oMi[l_elbow_Joint].translation().transpose();
}

Eigen::MatrixXd pseudoInv_SVD(const Eigen::MatrixXd &mat)
{
	typedef typename Eigen::MatrixXd::Scalar Scalar;
	auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
	const auto &singularValues = svd.singularValues();
	Eigen::MatrixXd singularValuesInv;
	double maxSgVal{0};
	for (unsigned int i = 0; i < singularValues.size(); ++i)
	{
		if (singularValues(i) > maxSgVal)
			maxSgVal = singularValues(i);
	}
	double tolerance = std::numeric_limits<double>::epsilon() * std::max(mat.rows(), mat.cols()) * maxSgVal;
	singularValuesInv = Eigen::MatrixXd::Zero(mat.cols(), mat.rows());
	for (unsigned int i = 0; i < singularValues.size(); ++i)
	{
		if (singularValues(i) > tolerance)
		{
			singularValuesInv(i, i) = Scalar{1} / singularValues(i);
		}
		else
		{
			singularValuesInv(i, i) = Scalar{0};
		}
	}
	return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
};

// 欧拉角转旋转矩阵，ZYX顺序
Eigen::Matrix<double, 3, 3> eul2Rot(double roll, double pitch, double yaw) {
	Eigen::Matrix<double,3,3> Rx,Ry,Rz;
	Rz<<cos(yaw),-sin(yaw),0,
			sin(yaw),cos(yaw),0,
			0,0,1;
	Ry<<cos(pitch),0,sin(pitch),
			0,1,0,
			-sin(pitch),0,cos(pitch);
	Rx<<1,0,0,
			0,cos(roll),-sin(roll),
			0,sin(roll),cos(roll);
	return Rz*Ry*Rx;
}
Eigen::Vector3d Rot2eul(const Eigen::Matrix3d &rot)
{
	Eigen::Vector3d eul;
	eul(0) = std::atan2(rot(2, 1), rot(2, 2));
	eul(1) = std::atan2(-rot(2, 0), std::sqrt(rot(2, 1) * rot(2, 1) + rot(2, 2) * rot(2, 2)));
	eul(2) = std::atan2(rot(1, 0), rot(0, 0));
	return eul;
}
double clip(double dataIn, double upperLim, double lowerLim){
	double data=dataIn;
	if (data>upperLim)
		data=upperLim;
	if (data<lowerLim)
		data=lowerLim;
	return data;
}

// 规整输入到上下界限之内，超出则通过加减2*3.1415进行调整
double adjustNumber(double num, double lowerBound, double upperBound) {
	if (num >= lowerBound && num <= upperBound) {
		return num;  // 已经在范围内，直接返回
	}

	// 如果数字小于a
	int k=0;
	if (num < lowerBound) {
		k = int(fabs(num-lowerBound)/(2*3.1415))+1;
		double res = num + k*2*3.1415;
		if (res <= upperBound) {  // 判断是否可以精确调整
			return res;  // 返回调整后的值
		}
		else
			return num;
	}
		// 如果数字大于b
	else if (num > upperBound) {
		k = int(fabs(num-upperBound)/(2*3.1415))+1;
		double res = num - k*2*3.1415;
		if (res >= lowerBound) {  // 判断是否可以精确调整
			return res;  // 返回调整后的值
		}
		else
			return num;
	}
	return num;
}